#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*/

#ifndef MovingKalman
#define MovingKalman
#endif

#define DRIVE_BIAS_FWD 1.02
#define DRIVE_BIAS_RGT 1

#define DRIVE_FACTOR_FWD_A 0.80 * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_B 0.88 * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_C 1    * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_D 0.68 * DRIVE_BIAS_FWD

#define DRIVE_FACTOR_RGT_A 0.80 * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_B 0.77 * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_C 1    * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_D 0.92 * DRIVE_BIAS_RGT

#define DRIVE_FACTOR_BCK_A 0.82
#define DRIVE_FACTOR_BCK_B 0.90
#define DRIVE_FACTOR_BCK_C 1
#define DRIVE_FACTOR_BCK_D 0.70

#define DRIVE_FACTOR_LFT_A 0.82
#define DRIVE_FACTOR_LFT_B 0.77
#define DRIVE_FACTOR_LFT_C 1
#define DRIVE_FACTOR_LFT_D 0.92

/*
 * @param degrees the degrees clockwise of "forward" for the direction of movement
 */
void moveDir(int degrees, int spd) {
  degrees = 90-degrees;
  degrees %= 360;

  float cosine_d = abs(cosDegrees(degrees));
  float sine_d = abs(sinDegrees(degrees));

  int robotDegrees = degrees - 45;
  float cosine_rob = cosDegrees(robotDegrees);
  float sine_rob = sinDegrees(robotDegrees);

  hogCPU();
  if (degrees>=0 && degrees < 90) { //1st quadrant
	  motor[a]=spd*sine_rob * ((DRIVE_FACTOR_FWD_A*sine_d) + (DRIVE_FACTOR_RGT_A*cosine_d));
	  motor[b]=spd*cosine_rob * ((DRIVE_FACTOR_FWD_B*sine_d) + (DRIVE_FACTOR_RGT_B*cosine_d));
	  motor[c]=spd*sine_rob * ((DRIVE_FACTOR_FWD_C*sine_d) + (DRIVE_FACTOR_RGT_C*cosine_d));
	  motor[d]=spd*cosine_rob * ((DRIVE_FACTOR_FWD_D*sine_d) + (DRIVE_FACTOR_RGT_D*cosine_d));
  }
  else if (degrees>=90 && degrees <180) { //2nd
	  motor[a]=spd*sine_rob * ((DRIVE_FACTOR_FWD_A*sine_d) + (DRIVE_FACTOR_LFT_A*cosine_d));
	  motor[b]=spd*cosine_rob * ((DRIVE_FACTOR_FWD_B*sine_d) + (DRIVE_FACTOR_LFT_B*cosine_d));
	  motor[c]=spd*sine_rob * ((DRIVE_FACTOR_FWD_C*sine_d) + (DRIVE_FACTOR_LFT_C*cosine_d));
	  motor[d]=spd*cosine_rob * ((DRIVE_FACTOR_FWD_D*sine_d) + (DRIVE_FACTOR_LFT_D*cosine_d));
  }
  else if (degrees>=180 && degrees <270) { //3rd
	  motor[a]=spd*sine_rob * ((DRIVE_FACTOR_BCK_A*sine_d) + (DRIVE_FACTOR_LFT_A*cosine_d));
	  motor[b]=spd*cosine_rob * ((DRIVE_FACTOR_BCK_B*sine_d) + (DRIVE_FACTOR_LFT_B*cosine_d));
	  motor[c]=spd*sine_rob * ((DRIVE_FACTOR_BCK_C*sine_d) + (DRIVE_FACTOR_LFT_C*cosine_d));
	  motor[d]=spd*cosine_rob * ((DRIVE_FACTOR_BCK_D*sine_d) + (DRIVE_FACTOR_LFT_D*cosine_d));
  }
  else { //4th
	  motor[a]=spd*sine_rob * ((DRIVE_FACTOR_BCK_A*sine_d) + (DRIVE_FACTOR_RGT_A*cosine_d));
	  motor[b]=spd*cosine_rob * ((DRIVE_FACTOR_BCK_B*sine_d) + (DRIVE_FACTOR_RGT_B*cosine_d));
	  motor[c]=spd*sine_rob * ((DRIVE_FACTOR_BCK_C*sine_d) + (DRIVE_FACTOR_RGT_C*cosine_d));
	  motor[d]=spd*cosine_rob * ((DRIVE_FACTOR_BCK_D*sine_d) + (DRIVE_FACTOR_RGT_D*cosine_d));
  }
  releaseCPU();

}

void adjustedDAS(int direction, int amount, int speed) {
  int i=0;
  nMotorEncoder[b] = 0;
  while (abs(nMotorEncoder[b]) < amount && abs(nMotorEncoder[c]) < amount) {
    writeDebugStreamLine(""+nMotorEncoder[b]);
    int facing = heading;
    if (facing > 180){
      facing -= 360;
    }
    moveDir(direction-facing,speed);
    wait1Msec(10);
  }
  stopmot();
}



void frontadjusted (int speed, int motorencgoing){
  nMotorEncoder[b] = 0;
  int facing = heading;
  if (facing > 180){
    facing -= 360;
  }
  while (abs(nMotorEncoder[b]) < motorencgoing){
    int difference = facing - heading;
    if (difference > 180){
      difference-= 360;
    }
    else if (difference < - 180){
      difference+= 360;
    }
    motor[a] = speed + difference;
    motor[b] = speed + difference;
    motor[c] = speed - difference;
    motor[d] = speed - difference;
  }
}

void rightadjusted (int speed, int motorencgoing){
  nMotorEncoder[b] = 0;
  int facing = heading;
  if (facing > 180){
    facing -= 360;
  }
  nMotorEncoder[b] = 0;
  while (abs(nMotorEncoder[b]) < motorencgoing){
    int difference = facing - heading;
    if (difference > 180){
      difference-= 360;
    }
    else if (difference < - 180){
      difference+= 360;
    }
    motor[a] = - speed + difference;
    motor[b] = speed + difference;
    motor[c] = -speed - difference;
    motor[d] = speed - difference;
  }
}

void leftadjusted (int speed, int motorencgoing){
  nMotorEncoder[b] = 0;
  int facing = heading;
  if (facing > 180){
    facing -= 360;
  }
  while (abs(nMotorEncoder[b]) < motorencgoing){
    int difference = facing - heading;
    if (difference > 180){
      difference-= 360;
    }
    else if (difference < - 180){
      difference+= 360;
    }
    motor[a] = speed + difference;
    motor[b] = - speed + difference;
    motor[c] = speed - difference;
    motor[d] = - speed - difference;
  }
}

void backadjusted (int speed, int motorencgoing){
  nMotorEncoder[b] = 0;
  int facing = heading;
  if (facing > 180){
    facing -= 360;
  }
  while (abs(nMotorEncoder[b]) < motorencgoing){
    int difference = facing - heading;
    if (difference > 180){
      difference-= 360;
    }
    else if (difference < - 180){
      difference+= 360;
    }
    motor[a] = - speed + difference;
    motor[b] = - speed + difference;
    motor[c] = - speed - difference;
    motor[d] = - speed - difference;
  }
}
